#include "KinectInterface.h"


using namespace cv;
using namespace std;

class Mutex {
public:
	Mutex() {
		pthread_mutex_init( &m_mutex, NULL );
	}
	void lock() {
		pthread_mutex_lock( &m_mutex );
	}
	void unlock() {
		pthread_mutex_unlock( &m_mutex );
	}
private:
	pthread_mutex_t m_mutex;
};

class MyFreenectDevice : public Freenect::FreenectDevice {
  public:
	MyFreenectDevice(freenect_context *_ctx, int _index)
		: Freenect::FreenectDevice(_ctx, _index), m_buffer_depth(FREENECT_DEPTH_11BIT_SIZE),m_buffer_rgb(FREENECT_VIDEO_RGB_SIZE), m_gamma(2048), m_new_rgb_frame(false), m_new_depth_frame(false),
		  depthMat(Size(640,480),CV_16UC1), rgbMat(Size(640,480),CV_8UC3,Scalar(0)), ownMat(Size(640,480),CV_8UC3,Scalar(0))
	{
		for( unsigned int i = 0 ; i < 2048 ; i++) {
			float v = i/2048.0;
			v = std::pow(v, 3)* 6;
			m_gamma[i] = v*6*256;
		}
	}
	// Do not call directly even in child
	void VideoCallback(void* _rgb, uint32_t timestamp) {
		//std::cout << "RGB callback" << std::endl;
		m_rgb_mutex.lock();
		uint8_t* rgb = static_cast<uint8_t*>(_rgb);
		rgbMat.data = rgb;
		m_new_rgb_frame = true;
		m_rgb_mutex.unlock();
	};
	// Do not call directly even in child
	void DepthCallback(void* _depth, uint32_t timestamp) {
		//std::cout << "Depth callback" << std::endl;
		m_depth_mutex.lock();
		uint16_t* depth = static_cast<uint16_t*>(_depth);
		depthMat.data = (uchar*) depth;
		m_new_depth_frame = true;
		m_depth_mutex.unlock();
	}

	bool getVideo(Mat& output) {
		m_rgb_mutex.lock();
		if(m_new_rgb_frame) {
			cv::cvtColor(rgbMat, output, CV_RGB2BGR);
			m_new_rgb_frame = false;
			m_rgb_mutex.unlock();
			return true;
		} else {
			m_rgb_mutex.unlock();
			return false;
		}
	}

	bool getDepth(Mat& output) {
			m_depth_mutex.lock();
			if(m_new_depth_frame) {
				depthMat.copyTo(output);
				m_new_depth_frame = false;
				m_depth_mutex.unlock();
				return true;
			} else {
				m_depth_mutex.unlock();
				return false;
			}
		}

  private:
	std::vector<uint8_t> m_buffer_depth;
	std::vector<uint8_t> m_buffer_rgb;
	std::vector<uint16_t> m_gamma;
	Mat depthMat;
	Mat rgbMat;
	Mat ownMat;
	Mutex m_rgb_mutex;
	Mutex m_depth_mutex;
	bool m_new_rgb_frame;
	bool m_new_depth_frame;
};



void KinectInterface::graphics(std::string &result){
    bool die(false);
    int iter = 0;

    /* Image processing variables */
    ImageProcessor imgProc;
    vector<Person> foundPeople;
    XmlWriter writer;

    /* Matrices for Kinect data */
	Mat depthMat(Size(640,480),CV_16UC1);
	Mat depthf  (Size(640,480),CV_8UC1);
	Mat rgbMat(Size(640,480),CV_8UC3,Scalar(0));

    /* Matrices for masking out background and foreground */
	Mat maskMat(Size(640,480),CV_8UC1);
	Mat bgMat(Size(640,480),CV_8UC1);
	Mat fgMat(Size(640,480),CV_8UC1);

	/* Matrices for rgb-separation */
	Mat r(Size(640,480),CV_8UC1,Scalar(0));
	Mat g(Size(640,480),CV_8UC1,Scalar(0));
	Mat b(Size(640,480),CV_8UC1,Scalar(0));

	vector<Mat> channels;
	channels.push_back(r);
	channels.push_back(g);
	channels.push_back(b);

    /* Kinect device */
	Freenect::Freenect<MyFreenectDevice> freenect;
    MyFreenectDevice *device;
    device = &freenect.createDevice(0);

	namedWindow("rgb",CV_WINDOW_AUTOSIZE);
	namedWindow("depth",CV_WINDOW_AUTOSIZE);

	device->startVideo();
	device->startDepth();
    while (!die) {
    	device->getVideo(rgbMat);
    	device->getDepth(depthMat);

    	depthMat.convertTo(depthf, CV_8UC1, 255.0/2048.0);

        split(rgbMat, channels); //Split channels of rgb image

    	fgMat = (depthf > 100); //Remove foreground
    	bgMat = (depthf < 120); //Remove background
    	maskMat = bgMat.mul(fgMat, 0.15); //Combine into mask


        GaussianBlur(maskMat, maskMat, Size(7,7), 5, 0, BORDER_DEFAULT); //Blur mask
    	channels[0] = channels[0].mul(maskMat, 0.003921569); //Use mask on blue channel
    	channels[1] = channels[1].mul(maskMat, 0.003921569); //Use mask on green channel
    	channels[2] = channels[2].mul(maskMat, 0.003921569); //Use mask on red channel


    	merge(channels, rgbMat); //Merge masked channels
        cv::imshow("rgb", rgbMat);
        //cv::imshow("depth",maskMat);

    	/*People detection */;
        foundPeople = imgProc.findPeople(channels[0], maskMat, depthf);

        /*End people detection*/


        /*Print result to xml*/
        TiXmlDocument doc;
        TiXmlDeclaration * decl = new TiXmlDeclaration( "1.0", "", "" );
        doc.LinkEndChild(decl);

        for (int i = 0; i<foundPeople.size(); i++){
            writer.writePerson(doc, foundPeople.at(i));
        }

        /*Print to result*/
        TiXmlPrinter printer;
        doc.Accept(&printer);
        doc.SaveFile("output.xml");
        result = printer.CStr();



		char k = cvWaitKey(5);
		if( k == 27 ){
		    cvDestroyWindow("rgb");
		    cvDestroyWindow("depth");
			break;
		}

		if(iter >= 1000) break;
		iter++;


    }

   	device->stopVideo();
	device->stopDepth();

}







